using System;

#if MF_FRAMEWORK_VERSION_V4_2
using Microsoft.SPOT;
using System.Collections;
#endif

namespace MLRobotic.Asserv.Utils
{
    public interface ILogger
    {
        bool IsEnabled { get; set; }
        void Write(string message);
        void AsservHeader(Pid dPid, Pid oPid);
        void Asserv(int d, int o, int v, int w, int setpointD, int setpointO, int setpointV, int setpointW,
                                  int commandV, int commandW, int pwmV, int pwmW, int leftPwm, int rightPwm, int errorD,
                                  int errorO);
        void Flush();
    }

    public static class Log
    {
        public static ILogger Instance = new InMemoryLogger();

        public static void Write(string message)
        {
#if MF_FRAMEWORK_VERSION_V4_2
            Debug.Print(message);
#endif
        }
    }

    public class InMemoryLogger : ILogger
    {
        private static readonly DateTime StartedTime = DateTime.Now;
        private static readonly ArrayList LogItems = new ArrayList();

        public static bool InMemory = false;

        public bool IsEnabled { get; set; }

        public void Write(string message)
        {
            Log.Write(message);
        }

        public void AsservHeader(Pid dPid, Pid oPid)
        {
            if (!IsEnabled) return;

            AsservLogItem.DisplayTitle(dPid, oPid);
            AsservLogItem.DisplayColumns();
        }

        public void Asserv(int d, int o, int v, int w, int setpointD, int setpointO, int setpointV, int setpointW, int commandV, int commandW, int pwmV, int pwmW, int leftPwm, int rightPwm, int errorD, int errorO)
        {
            if (!IsEnabled) return;

            if (InMemory)
                LogItems.Add(new AsservLogItem(DateTime.Now.Subtract(StartedTime).Ticks/TimeSpan.TicksPerMillisecond,
                                                d, o, v, w, setpointD, setpointO, setpointV, setpointW, commandV,
                                                commandW, pwmV, pwmW, leftPwm, rightPwm, errorD, errorO));
            else
                (new AsservLogItem(DateTime.Now.Subtract(StartedTime).Ticks/TimeSpan.TicksPerMillisecond, d, o, v, w,
                                   setpointD, setpointO, setpointV, setpointW, commandV, commandW, pwmV, pwmW, leftPwm,
                                   rightPwm, errorD, errorO)).DisplayItem();

        }

        public void Flush()
        {
            if (!IsEnabled) return;

            if (InMemory)
            {
                foreach (var item in LogItems)
                {
                    var log = (AsservLogItem) item;
                    log.DisplayItem();
                }

                LogItems.Clear();
            }
        }

        private class AsservLogItem
        {
            public long Ellapsed;
            public int D;
            public int O;
            public int V;
            public int W;
            public int SetpointD;
            public int SetpointO;
            public int SetpointV;
            public int SetpointW;
            public int CommandV;
            public int CommandW;
            public int PwmV;
            public int PwmW;
            public int LeftPwm;
            public int RightPwm;
            public int ErrorD;
            public int ErrorO;

            public AsservLogItem(long ellapsed, int d, int o, int v, int w, int setpointD, int setpointO, int setpointV, int setpointW, int commandV, int commandW, int pwmV, int pwmW, int leftPwm, int rightPwm, int errorD, int errorO)
            {
                this.Ellapsed = ellapsed;
                this.D = d;
                this.O = o;
                this.V = v;
                this.W = w;
                this.SetpointD = setpointD;
                this.SetpointO = setpointO;
                this.SetpointV = setpointV;
                this.SetpointW = setpointW;
                this.CommandV = commandV;
                this.CommandW = commandW;
                this.PwmV = pwmV;
                this.PwmW = pwmW;
                this.LeftPwm = leftPwm;
                this.RightPwm = rightPwm;
                this.ErrorD = errorD;
                this.ErrorO = errorO;
            }

            public static void DisplayTitle(Pid dPid, Pid oPid)
            {
                Log.Write(string.Concat("@Title:", "p: {", " Kp: ", dPid.Kp.ToString("F4"), ", Ki: ", dPid.Ki.ToString("F4"), ", Kd: ", dPid.Kd.ToString("F4"),
                    " }, o: {", " Kp: ", oPid.Kp.ToString("F4"), ", Ki: ", oPid.Ki.ToString("F4"), ", Kd: ", oPid.Kd.ToString("F4"), " }"));
            }

            public static void DisplayColumns()
            {
                Log.Write("@Columns:time;d;o;v;w;setpointD;setpointO;setpointV;setpointW;commandV;commandW;pwmV;pwmW;leftPwm;rightPwm;errorD;errorO");
            }

            public void DisplayItem()
            {
                Log.Write("@" + Ellapsed.ToString() + ";"
                + D.ToString() + ";" + O.ToString() + ";"
                + V.ToString() + ";" + W.ToString() + ";"
                + SetpointD.ToString() + ";" + SetpointO.ToString() + ";"
                + SetpointV.ToString() + ";" + SetpointW.ToString() + ";"
                + CommandV.ToString() + ";" + CommandW.ToString() + ";"
                + PwmV.ToString() + ";" + PwmW.ToString() + ";"
                + LeftPwm.ToString() + ";" + RightPwm.ToString() + ";"
                + ErrorD.ToString() + ";" + ErrorO.ToString());
            }
        }
    }
}
